/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file XMLtoData.cxx
	\brief class implemenation XML parser

	\author Jack Elston
	$Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "MAVParser.h"

	/*<-------Defines--------->*/
	/*<------End Defines------>*/

	/*<---Global Variables---->*/
		extern bool verbose;
		CommandQueue commandQueue;
	/*<-End Global Variables-->*/

MAVParser::MAVParser(String name) : ThreadedPipe(name)
{
}

void MAVParser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;									 // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);													// zero read fd set
	FD_ZERO(&writeFDs);													// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("MAVParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				commandQueue.push((uint8_t*)fifo_msg.data,n);

				while( !commandQueue.needMoreBytes() ) {
					if( commandQueue.isValid() ) {
						if(verbose)
							cout << "MAVParser::update() - valid command: ";

						Command cmd = commandQueue.popCommand();
						cmd.copyToBuf((uint8_t*)fifo_msg.data);

						if(verbose)
							printf("[%x]\n", CMD_CMD(fifo_msg.data));

						fifo_msg.src.s_addr = inet_addr("6.1.0.1");
						shmPtr->clients.add(fifo_msg.src);
						Client * a_client = shmPtr->clients.getClient(fifo_msg.src); // elstonj - temporary fix
						a_client->name = "MAV1";
						fifo_msg.size = cmd.size();
						writeFifo(INBOARD_TX);

					} else {
						commandQueue.pop();
					}
				}
			}
		}

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			readFifo(INBOARD_RX);
			writeFifo(OUTBOARD_TX);
		}
	}
}
